#include <Servo.h>

const int N_SERVOS = 18;

Servo servos[N_SERVOS];

struct servoSetup {
  byte pin;
  byte top;
  byte bottom;
  byte parkAngle;
  byte stand;
};

void park();
void attachServos();
void testCycle();
void detachServos();

Servo servo; 
int pos = 0;    // variable to store the servo position 
int servoPin;
int val;

#define STAND_ANGLE 90

#define HIP_0 15
#define SHOULDER_0 16
#define ELBOW_0 17

#define HIP_1 12
#define SHOULDER_1 13
#define ELBOW_1 14

#define HIP_2 9
#define SHOULDER_2 10
#define ELBOW_2 11

#define HIP_3 6
#define SHOULDER_3 7
#define ELBOW_3 8

#define HIP_4 3
#define SHOULDER_4 4
#define ELBOW_4 5

#define HIP_5 0
#define SHOULDER_5 1
#define ELBOW_5 2

const servoSetup servoConfig[N_SERVOS] = {
  // Limb 5
  { 2, 0, 180, 90, 90 },  // hip 0
  { 3, 0, 180 - STAND_ANGLE, 25, 180 - STAND_ANGLE },  // shoulder 1
  { 4, 180, 0, 60, 110 },  // elbow 2
  // Limb 4
  { 5, 0, 180, 90, 90 },  // hip 3
  { 6, 0, 180 - STAND_ANGLE, 25, 180 - STAND_ANGLE },  // shoulder 4
  { 7, 180, 0, 60, 110 },  // elbow 5
  // Limb 3
  { 8, 0, 180, 90, 90 },  // hip 6
  { 9, 0, 180 - STAND_ANGLE, 25, 180 - STAND_ANGLE },  // shoulder 7
  { 10, 180, 0, 60, 110 },  // elbow 8
  // Limb 2
  { 46, 0, 180, 90, 90 },  // hip 9
  { 48, 180, STAND_ANGLE, 155, STAND_ANGLE },  // shoulder 10
  { 50, 0, 180, 120, 82 },  // elbow 11
  // Limb 1
  { 40, 0, 180, 90, 90 },  // hip 12
  { 42, 180, STAND_ANGLE, 155, STAND_ANGLE },  // shoulder 13
  { 44, 0, 180, 120, 82 },  // elbow 14
  // Limb 0
  { 34, 0, 180, 90, 90 },  // hip 15
  { 36, 180, STAND_ANGLE, 155, STAND_ANGLE },  // shoulder 16
  { 38, 0, 180, 120, 82 },  // elbow 17
};

void setup() {
  Serial.begin(115200);
//  attachServos();
//  park();
//  delay(2000);
//  testCycle();
//  detachServos();
  
  Serial.begin(115200);
  servoPin = -1;
  val = 0;
}

void loop() {
    while (Serial.available()) {
    char c = Serial.read();
    Serial.print("c == ");
    Serial.println(c);
    
    if (c == 'p') {
      while (Serial.available()) Serial.read();
      Serial.println("Parking");
      attachServos();
      park();
      delay(1000);
      detachServos();
      return;     
    }

		if (c == 'r') {
      while (Serial.available()) Serial.read();
      Serial.println("Parking");
      attachServos();
			stand();
			delay(500);
			rippleStep(10);
      delay(1000);
      detachServos();
      return;			
		}

    if (c == 't') {
      while (Serial.available()) Serial.read();
      Serial.println("Testing");
      attachServos();
      park();
      delay(1000);
      testCycle();
      detachServos();
      return;
    }
    
    if (c == 's') {
      while (Serial.available()) Serial.read();
      Serial.println("Standing");
      attachServos();
      stand();
      delay(1000);
      detachServos();
      return;
    }

    
    if (c >= '0' && c <= '9') {
      val = (val * 10) + (c - '0');
    }
    if (c == ',') {
      servoPin = val;
      val = 0;
    }
    if (c == '\n') {
      if (val == 0)
        return; 
      if (servoPin == -1) {
        servoPin = val;
        val = -1;
      }

      Serial.print("Servo: ");
      Serial.print(servoPin);
      servo.attach(servoPin);
      if (val == -1) {
        for (int j = 0; j < 3; j++) {
          for(pos = 45; pos < 135; pos += 1) { // goes from 0 degrees to 180 degrees 
                                            // in steps of 1 degree 
            servo.write(pos);              // tell servo to go to position in variable 'pos' 
            delay(15);                       // waits 15ms for the servo to reach the position
          }
        } 
      } else {
        Serial.print(',');
        Serial.print(val);
        servo.write(val);
        delay(500);
      }
      Serial.println();
      servo.detach();
      servoPin = -1;
      val = 0;
    }
  }
}

void attachServos() {
  for (int i = 0; i < N_SERVOS; ++i) {
    servos[i].attach(servoConfig[i].pin);
  }
}

void detachServos() {
  for (int i = 0; i < N_SERVOS; ++i) {
    servos[i].detach();
  }
}

void park() {
  for (int i = 0; i < N_SERVOS; ++i) {
    servos[i].write(servoConfig[i].parkAngle);
  }
}

void testCycle() {
  Serial.println("Starting test cycle");

  for (int i = 0; i < N_SERVOS; ++i) {
    byte parkAngle = servoConfig[i].parkAngle;
    if (parkAngle < 90) {
      servos[i].write(parkAngle + 15);
    } else {
      servos[i].write(parkAngle - 15);
    }
    delay(300);
    servos[i].write(parkAngle);
    delay(300);
  } 
}

void stand() {
  park();
  delay(1000);

  for (int i = 0; i < 3; i++) {
    // 1,7,13
    servos[13].write(servoConfig[13].parkAngle);
    servos[14].write(servoConfig[14].parkAngle);
    servos[7].write(servoConfig[7].parkAngle);
    servos[8].write(servoConfig[8].parkAngle);
    servos[1].write(servoConfig[1].parkAngle);
    servos[2].write(servoConfig[2].parkAngle);
    delay(1000);
  
    servos[13].write(servoConfig[13].stand);
    servos[7].write(servoConfig[7].stand);
    servos[1].write(servoConfig[1].stand);
    servos[14].write(servoConfig[14].stand);
    servos[8].write(servoConfig[8].stand);
    servos[2].write(servoConfig[2].stand);
    delay(1000);
  
    // 4,10,16
    servos[4].write(servoConfig[4].parkAngle);
    servos[10].write(servoConfig[10].parkAngle);
    servos[16].write(servoConfig[16].parkAngle);
    servos[5].write(servoConfig[5].parkAngle);
    servos[11].write(servoConfig[11].parkAngle);
    servos[17].write(servoConfig[17].parkAngle);
    
    delay(1000);
  
    servos[4].write(servoConfig[4].stand);
    servos[10].write(servoConfig[10].stand);
    servos[16].write(servoConfig[16].stand);
    servos[5].write(servoConfig[5].stand);
    servos[11].write(servoConfig[11].stand);
    servos[17].write(servoConfig[17].stand);
    delay(1000);
  }
  delay(1000);
}

#define SHORT_PAUSE 25
#define LONG_PAUSE 250

void rippleStep(int numberOfSteps) {
	for (int i = 0; i < numberOfSteps; ++i) {
		// Lift the first leg
	  servos[ELBOW_0].write(servoConfig[ELBOW_0].parkAngle);
		delay(SHORT_PAUSE);
		servos[SHOULDER_0].write(servoConfig[SHOULDER_0].parkAngle);
		delay(LONG_PAUSE);
		// Rotate the hip
		servos[HIP_0].write(105);
		delay(LONG_PAUSE);
	  servos[ELBOW_0].write(servoConfig[ELBOW_0].stand);
		servos[SHOULDER_0].write(servoConfig[SHOULDER_0].stand);
		delay(LONG_PAUSE);

	  servos[ELBOW_1].write(servoConfig[ELBOW_1].parkAngle);
		delay(SHORT_PAUSE);
		servos[SHOULDER_1].write(servoConfig[SHOULDER_1].parkAngle);
		delay(LONG_PAUSE);
		// Rotate the hip
		servos[HIP_1].write(115);
		delay(LONG_PAUSE);
	  servos[ELBOW_1].write(servoConfig[ELBOW_1].stand);
		servos[SHOULDER_1].write(servoConfig[SHOULDER_1].stand);
		delay(LONG_PAUSE);

	  servos[ELBOW_2].write(servoConfig[ELBOW_2].parkAngle);
		delay(SHORT_PAUSE);
		servos[SHOULDER_2].write(servoConfig[SHOULDER_2].parkAngle);
		delay(LONG_PAUSE);
		// Rotate the hip
		servos[HIP_2].write(115);
		delay(LONG_PAUSE);
	  servos[ELBOW_2].write(servoConfig[ELBOW_2].stand);
		servos[SHOULDER_2].write(servoConfig[SHOULDER_2].stand);
		delay(LONG_PAUSE);

	  servos[ELBOW_3].write(servoConfig[ELBOW_3].parkAngle);
		delay(SHORT_PAUSE);
		servos[SHOULDER_3].write(servoConfig[SHOULDER_3].parkAngle);
		delay(LONG_PAUSE);
		// Rotate the hip
		servos[HIP_3].write(65);
		delay(LONG_PAUSE);
	  servos[ELBOW_3].write(servoConfig[ELBOW_3].stand);
		servos[SHOULDER_3].write(servoConfig[SHOULDER_3].stand);
		delay(LONG_PAUSE);

	  servos[ELBOW_4].write(servoConfig[ELBOW_4].parkAngle);
		delay(SHORT_PAUSE);
		servos[SHOULDER_4].write(servoConfig[SHOULDER_4].parkAngle);
		delay(LONG_PAUSE);
		// Rotate the hip
		servos[HIP_4].write(65);
		delay(LONG_PAUSE);
	  servos[ELBOW_4].write(servoConfig[ELBOW_4].stand);
		servos[SHOULDER_4].write(servoConfig[SHOULDER_4].stand);
		delay(LONG_PAUSE);

	  servos[ELBOW_5].write(servoConfig[ELBOW_5].parkAngle);
		delay(SHORT_PAUSE);
		servos[SHOULDER_5].write(servoConfig[SHOULDER_5].parkAngle);
		delay(LONG_PAUSE);
		// Rotate the hip
		servos[HIP_5].write(55);
		delay(LONG_PAUSE);
	  servos[ELBOW_5].write(servoConfig[ELBOW_5].stand);
		servos[SHOULDER_5].write(servoConfig[SHOULDER_5].stand);
		delay(LONG_PAUSE);

 		delay(LONG_PAUSE);
		delay(LONG_PAUSE);
	
		servos[HIP_0].write(servoConfig[HIP_0].stand);
		servos[HIP_1].write(servoConfig[HIP_1].stand);
		servos[HIP_2].write(servoConfig[HIP_2].stand);
		servos[HIP_3].write(servoConfig[HIP_3].stand);
		servos[HIP_4].write(servoConfig[HIP_4].stand);
		servos[HIP_5].write(servoConfig[HIP_5].stand);

    // 1,7,13
    servos[13].write(servoConfig[13].parkAngle);
    servos[14].write(servoConfig[14].parkAngle);
    servos[7].write(servoConfig[7].parkAngle);
    servos[8].write(servoConfig[8].parkAngle);
    servos[1].write(servoConfig[1].parkAngle);
    servos[2].write(servoConfig[2].parkAngle);
    delay(250);
  
    servos[13].write(servoConfig[13].stand);
    servos[7].write(servoConfig[7].stand);
    servos[1].write(servoConfig[1].stand);
    servos[14].write(servoConfig[14].stand);
    servos[8].write(servoConfig[8].stand);
    servos[2].write(servoConfig[2].stand);
    delay(250);
  
    // 4,10,16
    servos[4].write(servoConfig[4].parkAngle);
    servos[10].write(servoConfig[10].parkAngle);
    servos[16].write(servoConfig[16].parkAngle);
    servos[5].write(servoConfig[5].parkAngle);
    servos[11].write(servoConfig[11].parkAngle);
    servos[17].write(servoConfig[17].parkAngle);
    delay(250);
  
    servos[4].write(servoConfig[4].stand);
    servos[10].write(servoConfig[10].stand);
    servos[16].write(servoConfig[16].stand);
    servos[5].write(servoConfig[5].stand);
    servos[11].write(servoConfig[11].stand);
    servos[17].write(servoConfig[17].stand);
    delay(250);

		servos[HIP_0].write(70);
		servos[HIP_1].write(65);
		servos[HIP_2].write(65);
		servos[HIP_3].write(115);
		servos[HIP_4].write(115);
		servos[HIP_5].write(110);
  }
}
